/**
 * @ Author: luoqi
 * @ Create Time: 2024-10-13 22:33
 * @ Modified by: luoqi
 * @ Modified time: 2025-04-11 19:32
 * @ Description:
 */

#include <bsp.h>
#include <math.h>
#include "qdrive.h"
#include "../qshell/qterm.h"

static QDrive _drv;

static QCliCmd _cmd_qdrv;
static int _cmd_qdrv_hdl(int argc, char **argv);

static PmsmMotor _motor1 = {
    .poles_pairs = 7,
    .gear_ratio = 1.0f,
    .phase_inductance = 0.0025f,
    .phase_resistance = 16.5f,
    .rated_current = 0.8f,
    .stall_current = 1.6f,
    .rated_torque = 0.6f,
    .stall_torque = 1.75f,
    .rated_voltage = 24.0f,
    .rated_speed = 850.f,
    .kt = 0.188,
};
static PmsmMotor _motor2 = {
    .poles_pairs = 11,
    .gear_ratio = 1.0f,
    .phase_inductance = 0.0035f,
    .phase_resistance = 15.5f,
    .rated_current = 1.0f,
    .stall_current = 1.5f,
    .rated_torque = 0.3f,
    .stall_torque = 0.6f,
    .rated_voltage = 24.0f,
    .rated_speed = 300.f,
    .kt = 0.188,
};

int qdrv_init(uint8_t id)
{
    memset(&_drv, 0, sizeof(_drv));
    _drv.link_id = id;
    _drv.motor1 = _motor1;
    _drv.motor2 = _motor2;
    _drv.vbus_bias = 0.04;
    _drv.foc1_axis_aligned = false;
    _drv.foc2_axis_aligned = false;
    tx_event_flags_create(&_drv.foc_cloop_flags, "foc_cloop_flags");
    qterm_attach(&_cmd_qdrv, "qdrv", _cmd_qdrv_hdl, "qdrive commands");
    return 0;
}

QFocObj *foc_obj_get(FocDev dev)
{
    if(dev == FOC_DEV_1) {
        return &_drv.foc1;
    } else if(dev == FOC_DEV_2) {
        return &_drv.foc2;
    } else {
        return NULL;
    }
}

QDrive *qdrv_get()
{
    return &_drv;
}

int foc_pwm_ch_set(FocDev dev, PwmChannel ch, uint16_t value)
{
    TIM_HandleTypeDef *tim;
    if(dev == FOC_DEV_1) {
        tim = &htim1;
    } else if(dev == FOC_DEV_2) {
        tim = &htim8;
    } else {
        return -1;
    }
    uint32_t chn;
    switch(ch) {
    case PWM_CH1:
        chn = TIM_CHANNEL_1;
        break;
    case PWM_CH2:
        chn = TIM_CHANNEL_2;
        break;
    case PWM_CH3:
        chn = TIM_CHANNEL_3;
        break;
    default:
        return -1;
    }
    __HAL_TIM_SET_COMPARE(tim, chn, value);
    return 0;
}

int foc_pwm_set(FocDev dev, uint16_t pwma, uint16_t pwmb, uint16_t pwmc)
{
    if(dev == FOC_DEV_1) {
        uint16_t max = 0;
        if(pwma > pwmb) {
            max = pwma;
        } else {
            max = pwmb;
        }
        if(pwmc > max) {
            max = pwmc;
        }

        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pwma);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, pwmb);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, pwmc);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, (max + htim1.Init.Period) / 2);
        return 0;
    } else if(dev == FOC_DEV_2) {
        uint16_t max = 0;
        if(pwma > pwmb) {
            max = pwma;
        } else {
            max = pwmb;
        }
        if(pwmc > max) {
            max = pwmc;
        }
        __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, pwma);
        __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, pwmb);
        __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3, pwmc);
        __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_4, (max + htim8.Init.Period) / 2);
        return 0;
    } else {
        return -1;
    }
}

int foc_pwm_start(FocDev dev)
{
    if(dev == FOC_DEV_1) {
        HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
        HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
        HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
        HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
        HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
        HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, htim1.Init.Period / 2);
        return 0;
    } else if(dev == FOC_DEV_2) {
        HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);
        HAL_TIMEx_PWMN_Start(&htim8, TIM_CHANNEL_1);
        HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
        HAL_TIMEx_PWMN_Start(&htim8, TIM_CHANNEL_2);
        HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
        HAL_TIMEx_PWMN_Start(&htim8, TIM_CHANNEL_3);
        __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_4, htim8.Init.Period / 2);
        return 0;
    } else {
        return -1;
    }
}

int foc_pwm_stop(FocDev dev)
{
    if(dev == FOC_DEV_1) {
        HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
        HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_1);
        HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
        HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_2);
        HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
        HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_3);
        return 0;
    } else if(dev == FOC_DEV_2) {
        HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_1);
        HAL_TIMEx_PWMN_Stop(&htim8, TIM_CHANNEL_1);
        HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_2);
        HAL_TIMEx_PWMN_Stop(&htim8, TIM_CHANNEL_2);
        HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_3);
        HAL_TIMEx_PWMN_Stop(&htim8, TIM_CHANNEL_3);
        return 0;
    } else {
        return -1;
    }
}

static int _args_pwm_hdl(int argc, char **argv)
{
    UNUSED(argc);
    if(ISARG(argv[1], "on")) {
        foc_pwm_start(FOC_DEV_1);
        foc_pwm_start(FOC_DEV_2);
    } else if(ISARG(argv[1], "off")) {
        foc_pwm_stop(FOC_DEV_1);
        foc_pwm_stop(FOC_DEV_2);
    } else {
        return -1;
    }
    return 0;
}

static int _args_id_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" ID: %u\r\n", _drv.link_id);
    } else if(argc == 2) {
        _drv.link_id = atoi(argv[1]);
    } else {
        return -1;
    }
    return 0;
}

static QCliArgsEntry _args_qdrv_table[] = {
    { "pwm", 2, 2, _args_pwm_hdl, "<off/on>"},
    { "id", 1, 2, _args_id_hdl, "<id>"},
};

int _cmd_qdrv_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" VBUS: %f\r\n", _drv.vbus);
        QSH(" Temperature: %f\r\n", _drv.temperature);
        QSH(" ADC1_INJ_CNT: %u\r\n", _drv.adc1_inj_cnt);
        QSH(" ADC2_INJ_CNT: %u\r\n", _drv.adc2_inj_cnt);
        QSH(" ADC1_CONV_CNT: %u\r\n", _drv.adc1_conv_cnt);
        QSH(" ADC2_CONV_CNT: %u\r\n", _drv.adc2_conv_cnt);
        return 0;
    }
    if(ISARG(argv[1], "?") && argc == 2) {
        qterm_help(_args_qdrv_table, sizeof(_args_qdrv_table));
        return 0;
    }
    return qcli_args_handle(argc, argv, _args_qdrv_table, sizeof(_args_qdrv_table));
}
